module maxRPmod
	use basicmodule
	use setthmod
	use derivs

	implicit none
	
	contains
	
	subroutine maxfromx(niter,x,maxRP)	
		use bcong_int
		use u4inf_int
		use nnlpg_int
		integer		:: niter
		real		:: x(nx0)
		real		:: maxRP,maxth(nth)
		external	:: getnullrp, getnullrpgrad
		real		:: rparam(7)
		integer		:: iparam(7),j
		real		:: xlb(nx0),xub(nx0)

		call erset(0,0,0)
		call du4inf(iparam,rparam)
		iparam(3)=niter
		xlb=2*epsilon(1.0)**.33;xub=1-xlb
		j=0
		do
			call rnun(x)
			if(is_th_valid(getthfromx(x))) exit
			j=j+1
			if(mod(j,500)==0) then
				block
					integer	:: jj
					real	:: c(4), th(6)
					th=getthfromx(x)
					do jj=1,4
						c(jj)=getth_const(jj,th)
					enddo
					call mdisp(th)
					call mdisp(c)
				end block
				print *,"no valid parameter at tailflags:",tailflag
				call settailflag
			endif	
		enddo
		call nnlpg(getnullrp, getnullrpgrad,nthconst,0,0,xlb,xub,x=x,xguess=x,maxitn=niter,iprint=0)
		if(is_th_valid(getthfromx(x))) then
			maxRP=getRPfromx(x)
		else
			maxRP=-1.0
		endif
	end subroutine
	
	subroutine chksizeopt
		integer,parameter	::  ntries=5000
		real		:: maxRP
		real		:: RP, maxth(nth)
		real, allocatable	:: xs(:,:)
		integer		:: i
	
		allocate(xs(nx0,ntries))
		do i=1,ntries
			call rnun(xs(:,i))
		enddo
	
		maxRP=-1
		print *,"checking size by maximzing recection probability"
		tfcounter=1
		do i=1,ntries
			call settailflag
			call maxfromx(500,xs(:,i),RP)
			print *,tailflag,RP,getRPstddevclust(getthfromx(xs(:,i)))
			if(RP>maxRP) then
				print *,"new maximum", RP
				maxth=getthfromx(xs(:,i))
				call mdisp(xs(:,i).cvr.[maxth])
				call mdisp([getej(1,maxth(1:3)),getej(k,maxth(1:3)),getej(1,maxth(4:6)),getej(k,maxth(4:6))])
			endif
			maxRP=max(maxRP,RP)
		enddo
	end subroutine
	
	function getRPfromx(x) result(val)
		real	:: x(6),val
		val=getRP(getthfromx(x))
	end function
	
	function getRPfromxderiv(x) result(val)
		real	:: x(6),val(6),jac(6,6),xc(6)
		integer	:: i,j
		real, parameter	:: eps=epsilon(1.0)**(0.33)
		do j=1,6
			xc=x
			xc(j)=xc(j)+eps
			jac(:,j)=getthfromx(xc)
			xc(j)=xc(j)-2*eps
			jac(:,j)=jac(:,j)-getthfromx(xc)
		enddo
		jac=jac/(2*eps)
		val=matmul(getRPderiv(getthfromx(x)),jac)
	end function
	
	function getRPstddevclust(th) result(val)
		real	:: th(nth), val
		real	:: f0(2,npb,nblocks),p,pc(2)
		real	:: avtab(nblocks)
		integer	:: l,lb,i
		integer	:: l1,l2,lb2,i2
		integer(8)	:: code
		
!$omp parallel do private(l) collapse(2)
		do lb=1,nblocks
			do l=1,npb
				f0(1,l,lb)=getdens0(th(1:3),Y0s(:,l,lb))/densimp(l,lb)
				f0(2,l,lb)=getdens0(th(4:6),Y0s(:,l,lb))/densimp(l,lb)
			enddo
		enddo
		if(stage/=1) then
!$omp parallel do private(lb2,l1,l2,i2,code,pc,p)		
			do lb=1,nblocks
				lb2=lb+1
				if(lb2>nblocks) lb2=1
				p=0
				do l1=1,npb
					pc=0
					do i2=1,nbi
						l2=(i2-1)*nfpc+1
						code=ctest(i2,l1,lb)
						do 
							if(code<0) then
								pc(1)=pc(1)+f0(2,l2,lb2)		
								pc(2)=pc(2)+f0(1,l2,lb2)
							endif
							l2=l2+1
							code=code*2
							if(code==0) exit
						enddo
					enddo
					p=p+pc(1)*f0(1,l1,lb)+pc(2)*f0(2,l1,lb)
				enddo
				avtab(lb)=p/(2*npb**2)
			enddo
		else
!$omp parallel do private(lb2,l1,l2,i2,code,pc,p)		
			do lb=1,nblocks
				lb2=lb+1
				if(lb2>nblocks) lb2=1
				p=0
				do l1=1,npb
					pc=0
					do i2=1,nbi
						l2=(i2-1)*nfpc+1
						code=ctest(i2,l1,lb)
						if(code==0) cycle
						do 
							if(code<0) then
								pc(1)=pc(1)+f0(2,l2,lb2)		
							endif
							l2=l2+1
							code=code*2
							if(code==0) exit
						enddo
					enddo
					p=p+pc(1)*f0(1,l1,lb) 
				enddo
				avtab(lb)=p/(npb**2)
			enddo
		endif
			
		avtab=avtab-sum(avtab)/nblocks
		val=0
		do lb=1,nblocks
			lb2=lb+1
			if(lb2>nblocks) lb2=1
			val=val+avtab(lb)**2+2*avtab(lb)*avtab(lb2)
		enddo
		val=val/nblocks
		val=sqrt(val/nblocks)
	end function
	
	function getRP(th) result(val)
		real	:: th(nth), val
		real(4)	:: f0(2,npb,nblocks),p,pc(2)
		integer	:: l,lb,i
		integer	:: l1,l2,lb2,i2
		integer(8)	:: code
		
!$omp parallel do private(l) collapse(2)
		do lb=1,nblocks
			do l=1,npb
				f0(1,l,lb)=getdens0(th(1:3),Y0s(:,l,lb))/densimp(l,lb)
				f0(2,l,lb)=getdens0(th(4:6),Y0s(:,l,lb))/densimp(l,lb)
			enddo
		enddo
		p=0
		if(stage/=1) then					! exploit that tests that are symmetric the two Ys
!$omp parallel do private(lb2,l1,l2,i2,code,pc) reduction(+:p) 		
			do lb=1,nblocks
				lb2=lb+1
				if(lb2>nblocks) lb2=1
				do l1=1,npb
					pc=0
					do i2=1,nbi
						l2=(i2-1)*nfpc+1
						code=ctest(i2,l1,lb)
						if(code==0) cycle
						do 
							if(code<0) then
								pc(1)=pc(1)+f0(2,l2,lb2)		
								pc(2)=pc(2)+f0(1,l2,lb2)
							endif
							l2=l2+1
							code=code*2
							if(code==0) exit
						enddo
					enddo
					p=p+pc(1)*f0(1,l1,lb)+pc(2)*f0(2,l1,lb)
				enddo
			enddo
			val=p/(2*nsim*npb)
		else
!$omp parallel do private(lb2,l1,l2,i2,code,pc) reduction(+:p) 		
			do lb=1,nblocks
				lb2=lb+1
				if(lb2>nblocks) lb2=1
				do l1=1,npb
					pc=0
					do i2=1,nbi
						l2=(i2-1)*nfpc+1
						code=ctest(i2,l1,lb)
						if(code==0) cycle
						do 
							if(code<0) then
								pc(1)=pc(1)+f0(2,l2,lb2)		
							endif
							l2=l2+1
							code=code*2
							if(code==0) exit
						enddo
					enddo
					p=p+pc(1)*f0(1,l1,lb) 
				enddo
			enddo
			val=p/(nsim*npb)
		endif
	end function

	function getRPderiv(th) result(val)
		real	:: th(nth), val(6)
		real(4)	:: f0(2,npb,nblocks),f0d(3,2,npb,nblocks),p(6),pc(2),pcd(3,2)
		integer	:: l,lb,i
		integer	:: l1,l2,lb2,i2
		integer(8)	:: code
		
!$omp parallel do private(l) collapse(2)
		do lb=1,nblocks
			do l=1,npb
				f0(1,l,lb)=getdens0(th(1:3),Y0s(:,l,lb))/densimp(l,lb)
				f0d(:,1,l,lb)=f0(1,l,lb)*getdlog_dens0(th(1:3),Y0s(:,l,lb))
				f0(2,l,lb)=getdens0(th(4:6),Y0s(:,l,lb))/densimp(l,lb)
				f0d(:,2,l,lb)=f0(2,l,lb)*getdlog_dens0(th(4:6),Y0s(:,l,lb))
			enddo
		enddo
	
		p=0
		if(stage/=1) then
!$omp parallel do private(lb2,l1,l2,i2,code,pc,pcd) reduction(+:p)		
			do lb=1,nblocks
				lb2=lb+1
				if(lb2>nblocks) lb2=1
				do l1=1,npb
					pc=0;pcd=0
					do i2=1,nbi
						l2=(i2-1)*nfpc+1
						code=ctest(i2,l1,lb)
						do 
							if(code<0) then
								pc(1)=pc(1)+f0(2,l2,lb2)		
								pc(2)=pc(2)+f0(1,l2,lb2)
								pcd(:,1)=pcd(:,1)+f0d(:,2,l2,lb2)
								pcd(:,2)=pcd(:,2)+f0d(:,1,l2,lb2)
							endif
							l2=l2+1
							code=code*2
							if(code==0) exit
						enddo
					enddo
					p=p+[pc(1)*f0d(:,1,l1,lb)+f0(2,l1,lb)*pcd(:,2),pcd(:,1)*f0(1,l1,lb)+pc(2)*f0d(:,2,l1,lb)]
				enddo
			enddo
			val=p/(2*nsim*npb)
		else
!$omp parallel do private(lb2,l1,l2,i2,code,pc,pcd) reduction(+:p)		
			do lb=1,nblocks
				lb2=lb+1
				if(lb2>nblocks) lb2=1
				do l1=1,npb
					pc=0;pcd=0
					do i2=1,nbi
						l2=(i2-1)*nfpc+1
						code=ctest(i2,l1,lb)
						do 
							if(code<0) then
								pc(1)=pc(1)+f0(2,l2,lb2)		
								pcd(:,1)=pcd(:,1)+f0d(:,2,l2,lb2)
							endif
							l2=l2+1
							code=code*2
							if(code==0) exit
						enddo
					enddo
					p=p+[pc(1)*f0d(:,1,l1,lb),pcd(:,1)*f0(1,l1,lb)]
				enddo
			enddo
			val=p/(nsim*npb)
		endif
	end function
	
end module	
	
	
subroutine getnullrp(x,iact,val,ierr)
	use maxRPmod
	implicit none
	
	real	:: x(nth), val
	integer	:: iact
	logical	:: ierr
	real	:: RP

	
	if(iact==0) then
		val=-getRPfromx(x)
	else
		val=getth_const(iact,getthfromx(x))
	endif
	ierr=.not. isfinite(val)
end subroutine

subroutine getnullrpgrad(x,iact,val)
	use maxRPmod
	implicit none
	integer	:: iact
	real	:: x(nth), val(nth)
	logical	:: ierr
	integer	:: i
	real	:: xc(nth), v, vvec(nth)
	real, parameter	:: eps=epsilon(1.0)**(0.33)

	if(iact>0) then
!$omp parallel do private(xc,v) num_threads(nth)		
		do i=1,nth
			xc=x
			xc(i)=xc(i)+eps
			call getnullrp(xc,iact,vvec(i),ierr)
			xc(i)=xc(i)-2*eps
			call getnullrp(xc,iact,v,ierr)
			vvec(i)=vvec(i)-v
			xc(i)=xc(i)+eps
		enddo
		val=vvec/(2*eps)
	else
		val=-getRPfromxderiv(x)
	endif
	if(any(.not. isfinite(val))) val=0
end subroutine

